#include "CPathPlanRRT.h"

#include <random>
#include <iostream>

CPathPlanRRT::CPathPlanRRT(QObject *parent, \
                           double threshold, \
                           double close_goal_dis) :
                                                QObject(parent),
                                                m_threhold(threshold),
                                                m_close_goal_distance(close_goal_dis)
{
    m_rrt_tree = new CRRTTree (0.9, 50.0, 80.0);
    m_rrt_tree->setSizeX(1000);
    m_rrt_tree->setSizeY(1000);
}

CPathPlanRRT::CPathPlanRRT(double threshold, \
                           double close_goal_dis) :
                                                m_threhold(threshold),
                                                m_close_goal_distance(close_goal_dis)
{
    m_rrt_tree = new CRRTTree (0.9, 50.0, 100.0);
    m_rrt_tree->setSizeX(1000);
    m_rrt_tree->setSizeY(1000);
}

CPathPlanRRT::~CPathPlanRRT()
{
    delete m_rrt_tree;
}

void CPathPlanRRT::updateMap(const CGridMap &mapData)
{
    qDebug("[CPathPlanRRT] updateMap\n");
    quint32 size_x = mapData.getMapSizeX();
    quint32 size_y = mapData.getMapSizeY();

    m_size_x = size_x;
    m_size_y = size_y;

    m_rrt_tree->setSizeX(m_size_x);
    m_rrt_tree->setSizeY(m_size_y);

    m_step = 80;

    m_map_byte.resize(size_x * size_y);
    for (quint32 i = 0; i < size_x * size_y; i ++)
    {
        m_map_byte[i] = mapData[i];
    }
}

/**
 * @brief  rrt 规划
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2023.03.05
 */
bool CPathPlanRRT::planning()
{
    m_rrt_tree->clear();

    CPoint2D start_point(m_start_point.x(), m_start_point.y());

    CRRTNode start_node(start_point);
    m_rrt_tree->grow(start_node);
    CRRTNode *nod = m_rrt_tree->getNodeFrontPtr();
    nod->setParent(nod);

    qDebug("[CPathPlanRRT] planning\n");
    for (int i = 0; i < 10000; i ++)
    {
        bool ok;
        CRRTNode node = m_rrt_tree->generateTreeNode(ok);

        std::cout << "Tree Grow, " << i << " " << node << std::endl;

        if (!ok && isOccupied(node.getPoint()))
        {
            continue;
        }

        QPoint disp_point(node.getPoint().getX(), node.getPoint().getY());
        emit signalDisplayRandomPoint(disp_point);

        CPoint2D seg_start(node.getPoint().getX(), node.getPoint().getY());
        CPoint2D seg_end(node.getParent()->getPoint().getX(), node.getParent()->getPoint().getY());

        bool collision_state = checkSegmentCollision(seg_start, seg_end);

        if (!collision_state)
        {
            TNode disp_node;
            disp_node.point  = QPoint(seg_start.getX(), seg_start.getY());
            disp_node.parent = QPoint(node.getParent()->getPoint().getX(), node.getParent()->getPoint().getY());
            emit signalDisplayTreeNode(disp_node);

            m_rrt_tree->grow(node);

            bool close_state = isClosedDestPoint(seg_start);

            if (close_state)
            {
                CPoint2D dest_point(m_dest_point.x(), m_dest_point.y());
                bool collision_state = checkSegmentCollision(seg_start, dest_point);

                if (!collision_state)
                {
                    qDebug("[CPathPlanRRT] planning true\n");

                    CRRTNode dest_node(dest_point, m_rrt_tree->getLastNodePtr());
                    m_rrt_tree->grow(dest_node);

                    TNode disp_node;
                    disp_node.point  = QPoint(dest_point.getX(), dest_point.getY());
                    disp_node.parent = QPoint(seg_start.getX(), seg_start.getY());
                    emit signalDisplayTreeNode(disp_node);

                    std::vector<CPoint2D> path = m_rrt_tree->getPath();
                    QVector<QPoint> disp_path;
                    for (auto itr = path.rbegin(); itr != path.rend(); itr ++)
                    {
                        QPoint point((*itr).getX(), (*itr).getY());
                        disp_path.push_back(point);
                    }

                    emit signalDisplayOriginPath(disp_path);
                    return true;
                }
            }
        }

        QThread::msleep(100);
    }

    return false;
}

/**
 * @brief  判断点是否被占据
 * @param  point            
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2023.03.05
 */
bool CPathPlanRRT::isOccupied(const CPoint2D &point)
{
    if (isOutOfBound(point))
    {
        return true;
    }

    uint16_t x_pos = point.getX();
    uint16_t y_pos = point.getY();

    return (m_map_byte[y_pos * m_size_x + x_pos] == 0);
}

/**
 * @brief  判断是否超过边界
 * @param  point            
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2023.03.05
 */
bool CPathPlanRRT::isOutOfBound(const CPoint2D &point)
{
    if (point.getX() < 10 || point.getY() < 10 || point.getX() > m_size_x - 10 || point.getY() > m_size_y - 10)
    {
        return true;
    }

    return false;
}

/**
 * @brief  判断线段是否撞障碍物
 * @param  start_point      
 * @param  end_point        
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2023.03.05
 */
bool CPathPlanRRT::checkSegmentCollision(const CPoint2D &start_point, const CPoint2D &end_point)
{
    std::deque<CPoint2D> segement_points = CPoint2D::getPointBetweenPoints(1, start_point, end_point);

    for (int i = 0; i < (int)segement_points.size(); i ++)
    {
        if (isOccupied(segement_points.at(i)))
        {
            return true;
        }
    }

    return false;
}

/**
 * @brief  靠近目标点
 * @param  point            
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2023.03.05
 */
bool CPathPlanRRT::isClosedDestPoint(const CPoint2D &point)
{
    double distance = std::sqrt(std::pow(point.getX() - m_dest_point.x(), 2) + std::pow(point.getY() - m_dest_point.y(), 2));

    if (distance < 50.0)
    {
        return true;
    }

    return false;
}

